#include "drivers_motor.h"
#include "driver/gpio.h"
#include "driver/dac_oneshot.h"
#include <string.h>
int32_t DriversMotor::init(uint32_t _in1_pin, uint32_t _in2_pin, uint32_t _ref_pin)
{
    if (_in1_pin >= GPIO_NUM_MAX) {
        return -1;
    }

    if (_in2_pin >= GPIO_NUM_MAX) {
        return -2;
    }


    in1_pin = _in1_pin;
    in2_pin = _in2_pin;
    ref_pin = _ref_pin;


    gpio_config_t io_conf;
    memset(&io_conf, 0, sizeof(gpio_config_t));

    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pin_bit_mask = (1ULL << in1_pin);
    gpio_config(&io_conf);
    io_conf.pin_bit_mask = (1ULL << in2_pin);
    gpio_config(&io_conf);

    dac_oneshot_handle_t handle = nullptr;
    dac_oneshot_config_t dac_ch_cfg;
    dac_ch_cfg.chan_id = (dac_channel_t)ref_pin;
    
    ESP_ERROR_CHECK(dac_oneshot_new_channel(&dac_ch_cfg, &handle));

    dac_handle = (void *)handle;
    set(0, 0, 0);

    return 0;
}

void DriversMotor::set(uint32_t _in1_pin, uint32_t _in2_pin, uint32_t _ref_pin)
{
    set(_in1_pin, _in2_pin);
    dac_oneshot_output_voltage((dac_oneshot_handle_t)dac_handle, _ref_pin);
}


void DriversMotor::set(uint32_t _in1_pin, uint32_t _in2_pin)
{
    gpio_set_level((gpio_num_t)in1_pin, _in1_pin);
    gpio_set_level((gpio_num_t)in2_pin, _in2_pin);

}

void DriversMotor::set(uint32_t _ref_pin)
{
    dac_oneshot_output_voltage((dac_oneshot_handle_t)dac_handle, _ref_pin);
}

void DriversMotor::reset(void)
{
    set(0, 0, 0);
}


void DriversMotor::destroy(void)
{
    gpio_set_direction((gpio_num_t)in1_pin, GPIO_MODE_INPUT);
    gpio_set_direction((gpio_num_t)in2_pin, GPIO_MODE_INPUT);
}